/**
 * Marlin 3D Printer Firmware
 * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
 *
 * Based on Sprinter and grbl.
 * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 *
 */

#include "../inc/MarlinConfigPre.h"

#include "tool_change.h"

#include "motion.h"
#include "planner.h"
#include "temperature.h"

#include "../MarlinCore.h"
#include "../gcode/gcode.h"

//#define DEBUG_TOOL_CHANGE
//#define DEBUG_TOOLCHANGE_FILAMENT_SWAP


#if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
  migration_settings_t migration = migration_defaults;
#endif

#if ENABLED(TOOLCHANGE_FS_INIT_BEFORE_SWAP)
  Flags<EXTRUDERS> toolchange_extruder_ready;
#endif

#if ENABLED(TOOL_SENSOR)
  #include "../lcd/marlinui.h"
#endif

#if ENABLED(DUAL_X_CARRIAGE)
  #include "stepper.h"
#endif

#if HAS_LEVELING
  #include "../feature/bedlevel/bedlevel.h"
#endif

#if HAS_FANMUX
  #include "../feature/fanmux.h"
#endif

#if HAS_PRUSA_MMU1
  #include "../feature/mmu/mmu.h"
#elif HAS_PRUSA_MMU2
  #include "../feature/mmu/mmu2.h"
#endif

#if HAS_MARLINUI_MENU
  #include "../lcd/marlinui.h"
#endif

#if ENABLED(ADVANCED_PAUSE_FEATURE)
  #include "../feature/pause.h"
#endif

#if HAS_BED_PROBE
  #include "probe.h"
#endif

#if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
  #if TOOLCHANGE_FS_WIPE_RETRACT <= 0
    #undef TOOLCHANGE_FS_WIPE_RETRACT
    #define TOOLCHANGE_FS_WIPE_RETRACT 0
  #endif
#endif


// Move to position routines
void _line_to_current(const AxisEnum fr_axis, const float fscale=1) {
  line_to_current_position(planner.settings.max_feedrate_mm_s[fr_axis] * fscale);
}
void slow_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis, 0.2f); }
void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis, 0.5f); }

#define DEBUG_OUT ENABLED(DEBUG_TOOL_CHANGE)
#include "../core/debug_out.h"

inline void invalid_extruder_error(const uint8_t e) {
  SERIAL_ECHO_START();
  SERIAL_CHAR('T'); SERIAL_ECHO(e);
  SERIAL_CHAR(' '); SERIAL_ECHOLNPGM(STR_INVALID_EXTRUDER);
}
// zzg 使用电磁铁来驱动喷嘴的切换

//inline void est_activate_solenoid()   { OUT_WRITE(SOL0_PIN, HIGH); }// 如果用于笔式切割机，需要改成PWM输出
//inline void est_deactivate_solenoid() { OUT_WRITE(SOL0_PIN, LOW); }
//void est_init() { est_activate_solenoid(); }

/**
 * Perform a tool-change, which may result in moving the
 * previous tool out of the way and the new tool into place.
 */
void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
  /**
   * 1. 抬刀/笔，笔式绘图机没有Z轴，这里增加一个小的z运动，用于等待刀笔电磁铁抬落到位
   * 2. 一点新tool（刀笔）到就tool刚才的位置
   * 电磁铁的动作放在生成步进脉冲的定时中断中，从而在运动中抬落刀/笔，这样就不需要都等待运动完成了：planner.synchronize(); // 等待步进电机完成所有已规划运动
   */

  if (new_tool >= TOOL_MAX)
    return invalid_extruder_error(new_tool);

  const uint8_t old_tool = active_extruder;
  if (new_tool != old_tool) {
    const xy_pos_t diff = tool_offset[new_tool] - tool_offset[old_tool];
    DEBUG_POS("Start Tool-Change", current_position);

    // 1. 抬刀/笔
    current_position.z -= SWITCHING_TOOLHEAD_Z_HOP;
    DEBUG_POS("(1) Raise Z-Axis ", current_position);
    fast_line_to_current(Z_AXIS);

    // 2. 一点新tool（刀笔）到就tool刚才的位置
    DEBUG_SYNCHRONIZE();
    DEBUG_ECHOLNPGM("(2) Move to new tool", active_extruder);
    DEBUG_POS("Moving ParkPos", current_position);
    current_position.x -= diff.x;
    current_position.y -= diff.y;
    fast_line_to_current(X_AXIS);
    //est_deactivate_solenoid();

    DEBUG_POS("EMST Tool-Change done.", current_position);

    // Tell the planner the new "current position"
    //sync_plan_position();
  } // (new_tool != old_tool)

  SERIAL_ECHOLNPGM(STR_ACTIVE_EXTRUDER, active_extruder);

}


